Adaptive Covariance Estimation Method for LiDAR-Aided Multi-Sensor Integrated Navigation Systems

The accurate estimation of measurements covariance is a fundamental problem in sensors fusion algorithms and is crucial for the proper operation of filtering algorithms. This paper provides an innovative solution for this problem and realizes the proposed solution on a 2D indoor navigation system for unmanned ground vehicles (UGVs) that fuses measurements from a MEMS-grade gyroscope, speed measurements and a light detection and ranging (LiDAR) sensor. A computationally efficient weighted line extraction method is introduced, where the LiDAR intensity measurements are used, such that the random range errors and systematic errors due to surface reflectivity in LiDAR measurements are considered. The vehicle pose change is obtained from LiDAR line feature matching, and the corresponding pose change covariance is also estimated by a weighted least squares-based technique. The estimated LiDAR-based pose changes are applied as periodic updates to the Inertial Navigation System (INS) in an innovative extended Kalman filter (EKF) design. Besides, the influences of the environment geometry layout and line estimation error are discussed. Real experiments in indoor environment are performed to evaluate the proposed algorithm. The results showed the great consistency between the LiDAR-estimated pose change covariance and the true accuracy. Therefore, this leads to a significant improvement in the vehicle’s integrated navigation accuracy. OPEN ACCESS Micromachines 2015, 6 197